#include "node_lateral.h"
#include "common_tool/angle_helper.h"
#include "perpend_point2d_helper.h"
#include <cmath>
#include <limits>
#include <climits>
#include "common_tool/geometry.hpp"
#include "data_2d2s_path.h"
#include "common_tool/csv_writer.h"
#include <memory>
#include <chrono>

namespace auto_ros
{
namespace control
{
NodeLateral::NodeLateral(double loop_rate) : loop_rate_(loop_rate), rate_(loop_rate),
											 speed_controller_(0.2, 0.001, 0, 1 / loop_rate, -0.2, 0.2, -0.2, 0.2)

{

	localizition_dog_.setAllParameter(8, 1, 0, 3, 8);
	chassis_dog_.setAllParameter(8, 1, 0, 3, 8);
	planning_dog_.setAllParameter(8, 1, 0, 17, 25);
	//Linear2d2sMpc(const std::string dyn_config_filename, const std::string mpc_config_filename, double rate)
	std::string node_file_name = data_2d2s_path + "node_lateral.yaml";
	std::string dyn_file_name = data_2d2s_path + "dyn_lateral.yaml";
	std::string mpc_file_name = data_2d2s_path + "mpc_lateral.yaml";
	node_yaml_ = YAML::LoadFile(node_file_name);
	log_file_name_ = node_yaml_["output_csv"].as<std::string>();
	mpc_ptr_ = std::make_shared<VehicleLateralMpc>(dyn_file_name, mpc_file_name, loop_rate);
	unsigned int state_dim = mpc_ptr_->vehicle_dyn_ptr_->state_dim();
	unsigned int control_dim = mpc_ptr_->vehicle_dyn_ptr_->control_dim();
	unsigned int distur_dim = mpc_ptr_->vehicle_dyn_ptr_->distur_dim();
	//init mpc_ptr_ update_imput	Eigen::MatrixXd q_;
	x0_ = Eigen::VectorXd::Zero(state_dim);
	u_fb_ = Eigen::VectorXd::Zero(control_dim);
	distur_ref_ = Eigen::VectorXd::Zero(distur_dim);
	speed_ = 0;
	ref_lambda_ = 0;
}
void NodeLateral::CBLocalizition(const auto_msgs::localizition &msg)
{
	localizition_msg_ = msg;
	localizition_dog_.giveFood();
	//std::cout<<"************************"<<localizition_msg_.ax<<std::endl;
}
void NodeLateral::CBChassis(const auto_msgs::chassis &msg)
{
	chassis_msg_ = msg;
	chassis_dog_.giveFood();
}
void NodeLateral::CBPlanning(const auto_msgs::planning &msg)
{
	//ROS_INFO("[NodeLateral::CBPlanning] has received msg,msg size is:%d", msg.path.size());
	planning_msg_ = msg;
	planning_dog_.giveFood();
}
bool NodeLateral::SearchRefPoint(const auto_msgs::planning planning_msg) //search point relative to center of grativity
{
	auto_msgs::localizition localizition_msg = localizition_msg_;
	if (planning_msg.path.size() <= 0)
	{
		ROS_ERROR("[NodeLateral::SearchRefPoint] failed due to path size is zero");
		return false;
	}
	else
	{
		double lr = mpc_ptr_->vehicle_dyn_ptr_->lr();
		double heading_arc = localizition_msg.heading * 3.14 / 180;
		double x_cg = localizition_msg.x; //+ lr * std::cos(heading_arc);
		double y_cg = localizition_msg.y; //+ lr * std::sin(heading_arc);
		double last_closet_distance = std::numeric_limits<double>::max();
		double last_2nd_closet_distance = std::numeric_limits<double>::max();
		int closet_point_num = std::numeric_limits<int>::max();
		int second_closet_point_num = std::numeric_limits<int>::max();
		for (int i = 0; i < planning_msg.path.size(); i++) //search closet point to center of grativity
		{

			double path_x_cg_global = planning_msg.path[i].x - x_cg; //car cg point to path point vector in gloabal frame
			double path_y_cg_global = planning_msg.path[i].y - y_cg;
			//car cg point to path point vector in car frame
			double path_x_cg_car = path_x_cg_global * std::cos(heading_arc) +
								   path_y_cg_global * std::sin(heading_arc);
			double path_y_cg_car = -1.0 * path_x_cg_global * std::sin(heading_arc) +
								   path_y_cg_global * std::cos(heading_arc);
			double distance = std::pow(path_x_cg_car * path_x_cg_car + path_y_cg_car * path_y_cg_car, 0.5);
			if (distance < last_2nd_closet_distance)
			{
				if (distance <= last_closet_distance)
				{
					second_closet_point_num = closet_point_num;
					last_2nd_closet_distance = last_closet_distance;
					closet_point_num = i;
					last_closet_distance = distance;
				}
				else
				{
					second_closet_point_num = i;
					last_2nd_closet_distance = distance;
				}
			}
		}

		std::cout << "closet_point_num is " << closet_point_num << std::endl;
		std::cout << "second_closet_point_num is " << second_closet_point_num << std::endl;
		if (closet_point_num >= planning_msg.path.size() || second_closet_point_num >= planning_msg.path.size())
		{
			ROS_ERROR("[Node2d2s::SearchRefPoint] failed due to closet_point_num>path.size()");
			std::cout << "path size is:" << planning_msg.path.size() << std::endl;
			return false;
		}
		if (std::abs(closet_point_num - second_closet_point_num) > 5)
		{
			ROS_ERROR("[NodeLateral::SearchRefPoint] failed due to std::abs(closet_point_num-second_closet_point_num)>2 ");
			return false;
		}
		std::cout << "closet point is :" << planning_msg.path[closet_point_num] << std::endl;
		ref_point_closet_ = planning_msg.path[closet_point_num];
		ref_point_second_closet_ = planning_msg.path[second_closet_point_num];
		auto_msgs::planning_point vehicle_cg_point;
		vehicle_cg_point.x = x_cg;
		vehicle_cg_point.y = y_cg;
		if (!perpend_point2d<auto_msgs::planning_point>(vehicle_cg_point, ref_point_closet_,
														ref_point_second_closet_, ref_lambda_))
		{
			std::cout << "\033[perpend_point2d] in [NodeLateral::SearchRefPoint]failed  \033[0m"
					  << std::endl;
			return false;
		}

		else
		{
			if (std::abs(ref_lambda_) > 2)
				return false;
			//globol frame
			ref_point_interp_.x = ref_lambda_ * ref_point_closet_.x + (1 - ref_lambda_) * ref_point_second_closet_.x;
			ref_point_interp_.y = ref_lambda_ * ref_point_closet_.y + (1 - ref_lambda_) * ref_point_second_closet_.y;
			std::cout << "*******" << std::pow(std::pow(ref_point_interp_.x - x_cg, 2) + std::pow(ref_point_interp_.y - y_cg, 2.0), 0.5);
			//transform into car frame
			ref_point_interp_.x = ref_point_interp_.x - x_cg;
			ref_point_interp_.y = ref_point_interp_.y - y_cg;
			double temp_x = ref_point_interp_.x;
			ref_point_interp_.x = ref_point_interp_.x * std::cos(heading_arc) +
								  ref_point_interp_.y * std::sin(heading_arc);

			ref_point_interp_.y = -1.0 * temp_x * std::sin(heading_arc) +
								  ref_point_interp_.y * std::cos(heading_arc);
			ref_point_interp_.theta = auto_ros::common_tool::angle_mins_180to180(ref_lambda_ * auto_ros::common_tool::angle_mins_180to180(ref_point_closet_.theta - ref_point_second_closet_.theta) +
																				 ref_point_second_closet_.theta);
			ref_point_interp_.kappa = ref_lambda_ * ref_point_closet_.kappa +
									  (1 - ref_lambda_) * ref_point_second_closet_.kappa;
			ref_point_interp_.v = ref_lambda_ * ref_point_closet_.v +
								  (1 - ref_lambda_) * ref_point_second_closet_.v;
			ref_point_interp_.s = ref_lambda_ * ref_point_closet_.s +
								  (1 - ref_lambda_) * ref_point_second_closet_.s;
			ref_point_interp_.a = ref_lambda_ * ref_point_closet_.a +
								  (1 - ref_lambda_) * ref_point_second_closet_.a;
		}
	}

	return true;
}
bool NodeLateral::CheckDog()
{
	bool localizition_flag = false;
	bool chassis_flag = false;
	bool planning_flag = false;
	if (localizition_dog_.isDied())
	{
		localizition_flag = true;
		ROS_ERROR("NodeLateral::CheckDog():localizition_dog time out666666");
	}
	if (chassis_dog_.isDied())
	{
		chassis_flag = true;
		ROS_ERROR("NodeLateral::CheckDog():chassis_dog time out");
	}
	if (planning_dog_.isDied())
	{
		planning_flag = true;
		ROS_ERROR("NodeLateral::CheckDog():planning_dog time out");
	}
	if (localizition_flag || chassis_flag || planning_flag)
	{
		return false;
	}
	return true;
}
void NodeLateral::NoFood()
{
	localizition_dog_.noFood();
	chassis_dog_.noFood();
	planning_dog_.noFood();
}
void NodeLateral::HardBrake()
{
	control_cmd_.brake = 0;
	control_cmd_.throttle = 0;
	speed_controller_.resetIntergration();
}
bool NodeLateral::UpdateInput()
{
	double beta = 0;
	if (std::abs(localizition_msg_.vx) < 0.1)
	{
		beta = mpc_ptr_->vehicle_dyn_ptr_->lr() * last_steer_angle_ /
			   (mpc_ptr_->vehicle_dyn_ptr_->lf() + mpc_ptr_->vehicle_dyn_ptr_->lr());
	}
	else
	{
		beta = localizition_msg_.vy / localizition_msg_.vx;
	}
	double heading_error = auto_ros::common_tool::angle_mins_pi2pi(auto_ros::common_tool::angle_mins_180to180(localizition_msg_.heading) +
																   auto_ros::common_tool::angle_mins_180to180(beta * 180 / M_PI) - ref_point_interp_.theta);

	speed_ = chassis_msg_.speed_mps;
	double tire_f_angle = chassis_msg_.steering_angle_f / 180 * 3.14;
	if (tire_f_angle > mpc_ptr_->vehicle_dyn_ptr_->max_tire_f_angle())
	{
		tire_f_angle = mpc_ptr_->vehicle_dyn_ptr_->max_tire_f_angle();
	}
	if (tire_f_angle < -mpc_ptr_->vehicle_dyn_ptr_->max_tire_f_angle())
	{
		tire_f_angle = -mpc_ptr_->vehicle_dyn_ptr_->max_tire_f_angle();
	}
	x0_ << beta, localizition_msg_.avz / 180 * 3.14, -ref_point_interp_.y, heading_error;
	u_fb_ << last_steer_angle_ / 180 * M_PI;
	distur_ref_ << ref_point_interp_.kappa;
	if (node_yaml_["is_log"].as<int>() == 1)
	{
		double temp_speed = 1.0;
		if (std::abs(chassis_msg_.speed_mps) < 1.0)
		{
			if (chassis_msg_.speed_mps < 0)
				temp_speed = -1.0;
			else
				temp_speed = 1.0;
		}
		else
		{
			temp_speed = chassis_msg_.speed_mps;
		}
		static std::shared_ptr<io::CSVWriter> log_writer = std::make_shared<io::CSVWriter>(log_file_name_);
		//static io::CSVWriter x0_writer("x0_writer.csv");
		static std::vector<std::string> csv_header = {"beta", "beta_ref", "yaw_rate", "yaw_rate_ref", "e_y", "e_y_ref",
													  "e_heading", "e_heading_ref", "kappa", "tire_f_angle", "ref_lambda_",
													  "cruise_angle", "cruise_angle_ref", "x", "y", "y_ref", "alpha_f", "alpha_r", "f_angle_ref", "s", "speed"};
		static bool temp_bool = false;
		if (temp_bool == false)
		{
			log_writer->write_row<std::string>(csv_header);
			temp_bool = true;
		}
		std::vector<double> temp_vector = {x0_[0], mpc_ptr_->x_ref_(0), x0_[1], mpc_ptr_->x_ref_(1),
										   x0_[2], mpc_ptr_->x_ref_(2), x0_[3], mpc_ptr_->x_ref_(3),
										   ref_point_interp_.kappa,
										   tire_f_angle, ref_lambda_,
										   auto_ros::common_tool::angle_mins_180to180(auto_ros::common_tool::angle_mins_180to180(localizition_msg_.heading) + auto_ros::common_tool::angle_mins_180to180(localizition_msg_.az)),
										   ref_point_interp_.theta,
										   localizition_msg_.x,
										   localizition_msg_.y,
										   ref_lambda_ * ref_point_closet_.y + (1 - ref_lambda_) * ref_point_second_closet_.y,
										   180 / 3.14 * (x0_[0] + mpc_ptr_->vehicle_dyn_ptr_->lf() * x0_[1] / temp_speed - tire_f_angle),
										   180 / 3.14 * (x0_[0] - mpc_ptr_->vehicle_dyn_ptr_->lr() * x0_[1] / temp_speed),
										   mpc_ptr_->u_ref_(0), ref_point_interp_.s, temp_speed};
		log_writer->write_row<double>(temp_vector);
	}

	return true;
}
void NodeLateral::UpdateDebugMsg()
{
	//debug_msg_.tire_f_angle_max_ = tire_f_angle_max_;
	debug_msg_.lateral_speed = localizition_msg_.vy;
	debug_msg_.beta = x0_(0) * 180 / 3.14;
	debug_msg_.yaw_rate = x0_(1) * 180 / 3.14;
	debug_msg_.lateral_error = x0_(2);
	debug_msg_.heading_error = x0_(3) * 180 / 3.14;
	debug_msg_.kappa = ref_point_interp_.kappa;
	debug_msg_.lambda = ref_lambda_;
	debug_msg_.tire_f_angle_cmd = u_fb_(0) * 180 / 3.14;
	double temp_speed = 1.0;
	if (std::abs(chassis_msg_.speed_mps) < 1.0)
	{
		if (chassis_msg_.speed_mps < 0)
			temp_speed = -1.0;
		else
			temp_speed = 1.0;
	}
	else
	{
		temp_speed = chassis_msg_.speed_mps;
	}
	debug_msg_.f_alpha = (localizition_msg_.vy + mpc_ptr_->vehicle_dyn_ptr_->lf() * debug_msg_.yaw_rate) / temp_speed - debug_msg_.tire_f_angle_cmd;
	debug_msg_.r_alpha = (localizition_msg_.vy - mpc_ptr_->vehicle_dyn_ptr_->lr() * debug_msg_.yaw_rate) / temp_speed;
	debug_msg_.beta_ref = mpc_ptr_->x_ref_(0) * 180 / 3.14;
	debug_msg_.yaw_rate_ref = mpc_ptr_->x_ref_(1) * 180 / 3.14;
	debug_msg_.lateral_error_ref = mpc_ptr_->x_ref_(2);
	debug_msg_.heading_error_ref = mpc_ptr_->x_ref_(3) * 180 / 3.14;
	debug_msg_.f_angle_ref = mpc_ptr_->u_ref_(0) * 180 / 3.14;
}
bool NodeLateral::LateralCtr()
{
	auto start = std::chrono::high_resolution_clock::now();
	if (!mpc_ptr_->step(x0_, u_fb_, distur_ref_, speed_))
	{
		std::cout << "NodeLateral::LateralCtr failed,due to mpc_ptr_->step failed" << std::endl;
		return false;
	}
	auto end = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double, std::milli> tm = end - start;
	std::cout << "##########################" << tm.count() << std::endl;									// 毫秒
	control_cmd_.tire_angle_target_f = mpc_ptr_->vehicle_mpc_ptr_->get_contorl_one_shot(0)(0) * 180 / 3.14; //;vehicle_mpc_ptr_->get_optimal_ctr()(0, 0) / rate_ * 180 / 3.14;
	control_cmd_.tire_angle_target_r = 0;
	last_steer_angle_ = control_cmd_.tire_angle_target_f; //vehicle_mpc_ptr_->get_optimal_ctr()(1, 0) / rate_ * 180 / 3.14;
	return true;
}
bool NodeLateral::LongtitudeCtr()
{
	double current_speed = std::abs(chassis_msg_.speed_mps);
	//speed_cmd_ =std::min(speed_cmd_,15.0);
	speed_cmd_ = node_yaml_["test_speed"].as<double>(); //planning_msg_.path[planning_msg_.closet_point_num].v / 3.6*1.8;
	double speed_error = speed_cmd_ - current_speed;
	ROS_INFO("speed error is %f ", speed_error);
	ROS_INFO("speed_cmd_ is %f ", speed_cmd_);
	double temp_acc = speed_controller_.step(speed_error);
	if (temp_acc >= 0)
	{
		control_cmd_.throttle = std::abs(temp_acc);
		control_cmd_.brake = 0;
	}
	else
	{
		control_cmd_.throttle = 0;
		control_cmd_.brake = std::abs(temp_acc);
	}
	return true;
}
void NodeLateral::LoopProc()
{

	NoFood();
	if (!CheckDog())
	{
		HardBrake();
		control_cmd_pub_.publish(control_cmd_);
		return;
	}

	if (!SearchRefPoint(planning_msg_))
	{
		HardBrake();
		control_cmd_pub_.publish(control_cmd_);
		return;
	}

	UpdateDebugMsg();

	if (!UpdateInput())
	{

		HardBrake();
		control_cmd_pub_.publish(control_cmd_);
		return;
	}

	if (!LateralCtr())
	{
		//HardBrake();
		//control_cmd_pub_.publish(control_cmd_);
		//return;
	}

	LongtitudeCtr();
	control_cmd_pub_.publish(control_cmd_);
	debug_pub_.publish(debug_msg_);
}
} // namespace control
} // namespace auto_ros